/*
 * Copyright (c) 2025 STMROS Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "stmconn/stmconn_mock.hpp"
#include <iostream>
#include <thread>
#include <chrono>
#include <random>

namespace stmconn {

StmConnMock::StmConnMock()
  : stop_threads_(false), connected_(false) {
}

StmConnMock::~StmConnMock() {
  close();
}

bool StmConnMock::open() {
  if (connected_) {
    return true;
  }
  
  connected_ = true;
  stop_threads_ = false;
  
  // 启动模拟数据生成线程
  mock_thread_ = std::thread(&StmConnMock::mock_data_thread_func, this);
  
  std::cout << "Mock STM connection opened successfully" << std::endl;
  return true;
}

void StmConnMock::close() {
  {
    std::lock_guard<std::mutex> lock(mutex_);
    stop_threads_ = true;
    connected_ = false;
  }
  
  if (mock_thread_.joinable()) {
    mock_thread_.join();
  }
  
  if (closed_cb_) {
    closed_cb_();
  }
  
  std::cout << "Mock STM connection closed" << std::endl;
}

bool StmConnMock::is_open() const {
  return connected_;
}

bool StmConnMock::send_message(const StmLinkMessage & msg) {
  if (!connected_) {
    return false;
  }
  
  std::lock_guard<std::mutex> lock(mutex_);
  
  // 模拟发送消息，这里可以打印发送的消息内容
  std::cout << "Sending message to STM:" << std::endl;
  std::cout << "  System ID: " << static_cast<int>(msg.system_id) << std::endl;
  std::cout << "  Component ID: " << static_cast<int>(msg.component_id) << std::endl;
  std::cout << "  Message ID: " << msg.message_id << std::endl;
  std::cout << "  Payload size: " << msg.payload.size() << " bytes" << std::endl;
  
  if (!msg.payload.empty()) {
    std::cout << "  Payload: ";
    for (size_t i = 0; i < std::min(msg.payload.size(), size_t(10)); ++i) {
      std::cout << std::hex << static_cast<int>(msg.payload[i]) << " ";
    }
    if (msg.payload.size() > 10) {
      std::cout << "...";
    }
    std::cout << std::dec << std::endl;
  }
  
  return true;
}

void StmConnMock::mock_data_thread_func() {
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_int_distribution<> dis(1, 5);
  
  while (true) 
  {
      {
        std::lock_guard<std::mutex> lock(mutex_);
        if (stop_threads_) {
          break;
        }
      }
    
    // 生成模拟的心跳消息
    StmLinkMessage heartbeat_msg;
    heartbeat_msg.system_id = 1;
    heartbeat_msg.component_id = 1;
    heartbeat_msg.message_id = 0; // 心跳消息ID
    
    // 填充心跳消息内容
    heartbeat_msg.payload = {
      0x01, // type
      0x00, // autopilot
      0x00, // base_mode
      0x00, // custom_mode MSB
      0x00, // custom_mode LSB
      0x00, // system_status
      0x00  // stmlink_version
    };
    
    // 发送模拟的心跳消息
    if (receive_cb_) {
      receive_cb_(heartbeat_msg);
    }
    
    // 随机等待1-5秒
    std::this_thread::sleep_for(std::chrono::seconds(dis(gen)));
  } //end for while.
}

} // namespace stmconn